import time
import threading
import serial
import sys
import os
from djitellopy import Tello
from BrainLinkParser import BrainLinkParser   # ← 必须导入

# ==================== 配置区（根据需要微调） ====================
# BRAIN_PORT = "/dev/cu.BrainLink_Pro"     # macOS 示例
BRAIN_PORT = "COM5"                     # Windows 示例（改成你的实际 COM 口）

# 阈值（0-100，可根据你实际脑波表现调整）
ATTENTION_TAKEOFF = 85
MEDITATION_LAND = 85
FORWARD_THRESHOLD = 75
BACKWARD_THRESHOLD = 75
FLIP_LOW_BETA = 78
FLIP_HIGH_BETA = 78
CIRCLE_HIGH_GAMMA = 82

DEBOUNCE_SEC = 3.0      # 防误触冷却时间
RC_INTERVAL = 0.08      # RC控制发送间隔（推荐不要改太小）
# ===============================================================

latest_data = None
data_lock = threading.Lock()
is_flying = False
circling = False
last_action_time = 0
last_flip_time = 0
last_rc_time = 0

def on_eeg(data):
    """实时脑波回调"""
    global latest_data
    with data_lock:
        latest_data = data
    print(f"🧠 脑波 | 注意力:{data.attention:3d} 冥想:{data.meditation:3d} | "
          f"lowβ:{data.lowBeta:3d} highβ:{data.highBeta:3d} highγ:{data.highGamma:3d} | "
          f"δ:{data.delta:4d} θ:{data.theta:4d}")

def on_extend_eeg(data):
    print(f"📡 扩展 | 电量:{data.battery}%  温度:{data.temperature}°C")

def on_gyro(x, y, z): pass
def on_rr(rr1, rr2, rr3): pass
def on_raw(raw): pass

# BrainLink 串口读取线程
def serial_reading_thread(port):
    global parser
    try:
        ser = serial.Serial(port, 115200, timeout=1)
        print(f"✅ BrainLink 串口已打开: {port}")
        parser = BrainLinkParser(on_eeg, on_extend_eeg, on_gyro, on_rr, on_raw)
        
        while True:
            if ser.in_waiting > 0:
                byte_data = ser.read(ser.in_waiting)
                if byte_data:
                    parser.parse(byte_data)
            time.sleep(0.01)
    except Exception as e:
        print(f"❌ 串口错误: {e}")
        sys.exit(1)

if __name__ == "__main__":
    # 确保 BrainLinkParser 能被 import
    sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
    
    # 启动 BrainLink 读取线程
    threading.Thread(target=serial_reading_thread, args=(BRAIN_PORT,), daemon=True).start()
    
    # 初始化 Tello
    tello = Tello()
    tello.connect()
    print(f"✅ Tello 已连接，当前电量: {tello.get_battery()}%")
    
    print("\n🚀 增强版脑控无人机已就绪！")
    print("   注意力 ≥85     → 起飞 / 前进")
    print("   冥想 ≥85       → 降落 / 后退")
    print("   lowBeta ≥78    → 左翻滚")
    print("   highBeta ≥78   → 右翻滚")
    print("   highGamma ≥82  → 切换【绕飞模式】")
    print("   Ctrl+C         → 安全退出\n")
    
    try:
        while True:
            current_time = time.time()
            
            with data_lock:
                if latest_data is None:
                    time.sleep(0.1)
                    continue
                d = latest_data
                att = d.attention
                med = d.meditation
                low_beta = d.lowBeta
                high_beta = d.highBeta
                high_gamma = d.highGamma
            
            # ==================== 起飞 / 降落（一次性动作） ====================
            if current_time - last_action_time > DEBOUNCE_SEC:
                if att >= ATTENTION_TAKEOFF and not is_flying:
                    print("🛫 高注意力 → 执行【起飞】")
                    tello.takeoff()
                    is_flying = True
                    circling = False
                    last_action_time = current_time
                    
                elif med >= MEDITATION_LAND and is_flying and not circling:
                    print("🛬 高冥想 → 执行【降落】")
                    tello.land()
                    is_flying = False
                    circling = False
                    last_action_time = current_time
            
            # ==================== 飞行中平滑控制（RC 控制） ====================
            if is_flying and current_time - last_rc_time > RC_INTERVAL:
                fb = 0   # 前后速度
                yaw = 0  # 偏航（用于绕飞）
                
                # 前进 / 后退（速度与脑波强度正相关）
                if att > FORWARD_THRESHOLD:
                    fb = int((att - FORWARD_THRESHOLD) * 1.8)
                    fb = min(fb, 70)
                elif med > BACKWARD_THRESHOLD:
                    fb = -int((med - BACKWARD_THRESHOLD) * 1.6)
                    fb = max(fb, -60)
                
                # 绕飞模式（持续小圈飞行）
                if circling:
                    yaw = 45          # 顺时针偏航
                    fb = max(fb, 25)  # 保持轻微前进，形成圆形轨迹
                
                # 发送 RC 控制指令（左右=0，上下=0）
                tello.send_rc_control(0, fb, 0, yaw)
                last_rc_time = current_time
            
            # ==================== 翻滚（一次性动作） ====================
            if is_flying and current_time - last_flip_time > DEBOUNCE_SEC + 1:
                if low_beta >= FLIP_LOW_BETA:
                    print("🔄 lowBeta 高 → 执行【左翻滚】")
                    tello.flip_left()
                    last_flip_time = current_time
                elif high_beta >= FLIP_HIGH_BETA:
                    print("🔄 highBeta 高 → 执行【右翻滚】")
                    tello.flip_right()
                    last_flip_time = current_time
            
            # ==================== 切换绕飞模式 ====================
            if is_flying and high_gamma >= CIRCLE_HIGH_GAMMA and current_time - last_action_time > DEBOUNCE_SEC:
                circling = not circling
                print(f"{'🌀 启动【绕飞模式】' if circling else '⭕ 停止绕飞'}")
                last_action_time = current_time
            
            time.sleep(0.05)
    
    except KeyboardInterrupt:
        print("\n⛔ 用户中断 → 安全停止...")
        if is_flying:
            tello.send_rc_control(0, 0, 0, 0)
            try:
                tello.land()
            except:
                pass
        print("程序已安全退出！")